#include "VisionNode.h"

#include "../utilities/ObjPosition.h"


ObjPosition global_pos(0,0,0.0);

//static const char WINDOW[] = "Image window"; // TODO

sensor_msgs::Image::ConstPtr rgb_global;
sensor_msgs::Image::ConstPtr depth_global;

VisionNode::VisionNode():m_rgb_trans(m_node_handler){

	// for debugging (see image):
    m_image_pub = m_rgb_trans.advertise("out", 1);

    m_rgb_sub = m_node_handler.subscribe("/camera/rgb/image_color", 1, &VisionNode::detectObjCB, this);

    // use webcam:
    // m_image_sub = it_.subscribe("/image_raw", 1, &VisionNode::detectObjCB, this);

    // publishes the object position
	m_obj_pos_publisher = m_node_handler.advertise<std_msgs::String>("/VisionNode/ObjPosition", 1000);
 }


void VisionNode::detectObjCB(const sensor_msgs::Image::ConstPtr& msg) {

 // acquire RGB image from camera
	cv_bridge::CvImagePtr cv_ptr;
    try {
      cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    IplImage input(cv_ptr->image);
    IplImage* img = cvCloneImage(&input);

    // create HSV image out of RGB
    IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);
    cvCvtColor(img, imgHSV, CV_BGR2HSV);

    // threshold HSV image by object color
    IplImage* imgThreshed = cvCreateImage(cvGetSize(img), 8, 1);
  
    // green ball
    cvInRangeS(imgHSV,cvScalar(50,25,25), cvScalar(75,255,255),imgThreshed); // TODO CONSTANTS

    RNG rng(12345);

    // cast thresholded image to iterable matrix format
    Mat src = Mat(imgThreshed, true);

    // find contours
    std::vector<std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(src, contours, hierarchy, CV_RETR_TREE,
        CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));

    // get the moments

    Point2f p2f;
    double area = 0.0;
    double max_area = 0.0;
    int max_index = -1;
    for (unsigned int i = 0; i < contours.size(); ++i) {

      area = contourArea(contours[i]); // calculate area for current contour

      // skip insignificant contours
      if (area <= 25) { // TODO CONSTANTS
        continue;
      }

      // find greatest contour (it describes the object)
      if (area > max_area) {
        max_area = area;
        max_index = i;
      }
    }

	static double dist = 0.0;
	static int pubX = 0, pubY = 0;

    // object was not detected on current frame
    if (max_index < 0) {

    	ROS_ERROR("max_index = %d", max_index); // TODO DEBUG PRINT!!!

    	// indicate that no object was found
    	pubX = -1; // TODO CONSTANTS
    	pubY = -1;
    	dist = -1.0;

		// set values of object position
    	safeObjPosSet(pubX, pubY, dist);

    } else {

		// find object's center of mass
		Moments mu = moments(contours[max_index], false);
		p2f = Point2f(mu.m10 / mu.m00, mu.m01 / mu.m00);

		ROS_INFO("p2f = [x,y] = [%d,%d]", int(p2f.x), int(p2f.y)); // TODO DEBUG PRINT!!!

    	// update object position
    	pubX = int(p2f.x);
    	pubY = int(p2f.y);
    	dist = max_area;

		// set values of object position
    	safeObjPosSet(pubX, pubY, dist);
    }

    // release allocated memory for images
    cvReleaseImage(&img);
    cvReleaseImage(&imgHSV);
    cvReleaseImage(&imgThreshed);

//    cv::waitKey(3); // TODO FIND OUT MEANING

    // compose message
	char buffer[16]; // TODO RESET DATA AFTER EACH CALL TO CALLBACK, CONSTANTS
	snprintf(buffer, sizeof(buffer), "%d%s%d%s%3.3f",pubX,DELIMITER,pubY,DELIMITER,dist);
	m_message_to_publish.data = buffer;

	// publish message with object position
	m_obj_pos_publisher.publish(m_message_to_publish);
}

void VisionNode::safeObjPosSet(int x, int y, double dist) {

	std::unique_lock<std::mutex> mlock(mutex_);

	global_pos.m_x = x;
	global_pos.m_y = y;
	global_pos.m_dist = dist;

	ROS_ERROR("global_pos.m_x = %d, global_pos.m_y = %d, dist = %f", global_pos.m_x, global_pos.m_y,global_pos.m_dist);

	mlock.unlock();
}

VisionNode::~VisionNode() {

}



int main(int argc, char** argv) {
  ros::init(argc, argv, "VisionNode");

  VisionNode vn;

  ros::spin();
  return 0;
}

// Colors:

// cvInRangeS(imgHSV,cvScalar(10,100,210), cvScalar(30,105,255),imgThreshed);

// orange-red
// cvInRangeS(imgHSV,cvScalar(10,135,155), cvScalar(20,255,255),imgThreshed);

// bright green:
// cvInRangeS(imgHSV,cvScalar(53,74,160), cvScalar(90,147,255),imgThreshed);

// Mat
// circle( src, Point(50,50), 100, Scalar(255,0,0), 3, 8, 0 );
